clc;clear;close all;
%电机参数表
motor.Ra = 2.49;            %电枢电阻
motor.La = 6.1 * 10^(-4);   %电枢电感
motor.Ka = 8.22 * 10^(-2);  %转矩常数
motor.Ke = 8.24 * 10^(-2);  %感应电动势常数
motor.Ir = 1.19 * 10^(-5);  %转子惯量
motor.Br = 4.10 * 10^(-4);  %转子阻尼
motor.m = 0.5;              %连杆质量
motor.L = 0.1;              %连杆质心距离转轴距离
motor.I1 = 5.0 * 10^(-3);   %负载惯量
motor.B1 = 0.2;             %关节阻尼
motor.Ku = 3;               %电压增益
motor.Kg = 1;               %跨导增益
motor.g = 9.8;              %重力加速度
%设n=50 t=1s
n = 50;                         %传动比
motor.Im = motor.Ir + motor.I1/(n^2);             %等效转动惯量
motor.Bm = motor.Br + motor.B1/(n^2);             %等效阻尼

motor.Tmv = (motor.Ra*motor.Im)/(motor.Ke*motor.Ka + motor.Ra*motor.Bm);  %时间常数
motor.Kmv = (motor.Ka*motor.Ku)/(motor.Ke*motor.Ka + motor.Ra*motor.Bm);  %开环增益
motor.Kdv =  1/((motor.Ke*motor.Ka/motor.Ra) + motor.Bm);     %干扰增益

step_time = 0.0001;          %仿真步长
stop_time = 1;             %仿真停止时间
size = int32(stop_time/step_time);
Beta_expect = 1;             %设定加速度
%生成位置S曲线
[omega_expect, theta_expect] = Postion_S_Curve(stop_time, step_time, Beta_expect);

t=0:step_time:stop_time;         %仿真时间
w=zeros(size,1);    %角速度1
theta = zeros(size,1);    %当前角度
Err_w_sum = 0;                %误差角速度累计
 %PID参数
Kp = 50;
Kv = 1000;                    
Tv = 0.001;

for i = 1:length(t)-1       %PID环
    [w(i+1),theta(i+1), Err_w_sum] = PID_Pos_Loop(w(i), theta(i), Err_w_sum, motor, n, Kp, Kv, Tv, 0, theta_expect(i), step_time);
end

%绘制速度及位置曲线
figure(1);
plot(t,theta,'r');
ylabel('theta(rad)');
hold on;
plot(t,theta_expect,'b');
legend('位置实际值', '位置期望值');
title('位置响应曲线');

figure(2);
plot(t,w,'r');
ylabel('omega(t)(rad/s)');
hold on;
plot(t,omega_expect,'b');
legend('角速度实际值', '角速度期望值');
title('速度响应曲线');

